%% 例题6-4：针对平面2R机器人，可设定轨迹类型和减速比的逆动力学PID控制器仿真例程
% 根据题义，假定两个关节电机和系统参数均相同的情况，因此，非线性力矩计算公式不具有通用性
% 利用循环模拟控制系统时钟，假定每个采样周期都进行伺服控制，迭代计算电机速度，且每次伺服控制都更新伺服期望值定时器
%% 程序启动，清理内存和绘图
clc;
clear;
clf;

%% 定义和设定控制相关变量
%% 定义跟踪轨迹曲线类型符号
Hold_on = 0;
Traj_Slope = 1;
Traj_Pos_S = 2;
Traj_Vol_S = 3;
%% 选择跟踪轨迹
Traj_Type = Hold_on;
% Traj_Type = Traj_Slope;
% Traj_Type = Traj_Pos_S;
% Traj_Type = Traj_Vol_S;

%% 设定减速比
N = 12; 

%% 设定电机力矩上限
ia_limit = 2.5; % 设定电机电流上限为2.5A
Ka = 8.22e-2; % 电机力矩常数
Kg = 1; % 电流放大器增益
J_Tq_limit = ia_limit * Ka * Kg * N; % 设定关节力矩上限

%% 设定闭环系统响应特性
ts = 0.1;%调节时间
Zeta = 1;%阻尼比
if (Zeta == 1), Omega_n = 4.75/ts; end % 临界阻尼系统的自然频率
if (Zeta < 1), Omega_n = 3.5/(Zeta*ts); end % 欠阻尼系统的自然频率
if (Zeta > 1), Omega_n = 3.3/((Zeta- sqrt(Zeta^2-1))*ts); end % 过阻尼系统的自然频率

%% 机器人PID控制器增益矩阵
Kp = [Omega_n^2, 0; 0, Omega_n^2];
Kv = [2*Zeta*Omega_n, 0; 0, 2*Zeta*Omega_n];
Ki = [1000, 0; 0, 1000];

%% 设定模型偏差
D_m = 0.05; % 理论质量与实际质量的偏差率
D_L = 0.05; % 理论杆长与实际杆长的偏差率

%% 系统参数初始化
%% 单杆参数
g = 9.8;
m = 0.5; % 实际质量
L = 0.1; % 实际杆长
Br = 4.1e-4;
Ir = 1.19e-5;
Bl = 2e-2;
m_D = m * (1 - D_m); % 理论质量
L_D = L * (1 - D_L); % 理论杆长

%% 平面2R机器人驱动空间参数矩阵，用于模拟真实关节运动
% 平均等效质量矩阵，实际值
M11_Average = (3*m*L^2)/N^2 + Ir; % 1#关节电机平均等效转动惯量
M22_Average = (m*L^2)/N^2 + Ir; % 2#关节电机平均等效转动惯量
Mm_Average = [M11_Average 0;0 M22_Average]; % 机器人驱动空间平均惯量矩阵
% 等效阻尼矩阵
Bm11 = Bl/N^2 + Br; % 1#关节电机平均等效转动惯量
Bm22 = Bl/N^2 + Br; % 2#关节电机平均等效转动惯量
Bm = [Bm11 0; 0 Bm22]; % 机器人驱动空间等效阻尼矩阵

%% 设置仿真时间、步长和步数
dt = 0.0001;
if (Traj_Type == Hold_on), total_time = 1;end
if (Traj_Type == Traj_Slope), total_time = 2;end
if (Traj_Type == Traj_Pos_S), total_time = 4;end
if (Traj_Type == Traj_Vol_S), total_time = 7;end
time = 0:dt:total_time; % 时间序列
step = total_time/dt + 1; % 步数

%% 初始化保存仿真计算结果的数组，均为2行、step列矩阵，行数代表关节编号，列数为迭代步数
J_theta = zeros(2,step); % 记录关节实际转角数组，输出图形的纵坐标轴
J_omega = zeros(2,step); % 记录关节实际转速，输出图形的纵坐标轴
J_epsilon = zeros(2,step); % 记录关节实际加速度，输出图形的纵坐标轴

%% 初始化仿真过程的中间变量，均为2行、1列，行数代表关节编号，这里根据题义，假定两关节均从0°位置零速启动
J_real_theta_pre = zeros(2,1); % 迭代计算前，当前关节实际转角
J_real_theta_aft = zeros(2,1); % 迭代计算后，当前关节实际转角
J_real_omega_pre = zeros(2,1); % 迭代计算前，当前关节实际转速
J_real_omega_aft = zeros(2,1); % 迭代计算后，当前关节实际转速
J_real_epsilon_pre = zeros(2,1); % 迭代计算后，当前关节实际加速度
J_real_epsilon_aft = zeros(2,1); % 迭代计算后，当前关节实际加速度
M_real_theta_pre = zeros(2,1); % 迭代计算前，当前电机实际转角
M_real_theta_aft = zeros(2,1); % 迭代计算后，当前电机实际转角
M_real_omega_pre = zeros(2,1); % 迭代计算前，当前电机实际转速
M_real_omega_aft = zeros(2,1); % 迭代计算后，当前电机实际转速
M_real_epsilon_pre = zeros(2,1); % 迭代计算后，当前电机实际加速度
M_real_epsilon_aft = zeros(2,1); % 迭代计算后，当前电机实际加速度

J_Tqc = zeros(2,1); % 控制器输出的关节空间控制力矩
M_Tqd = zeros(2,1); % 等效到电机转子的扰动力矩
M_Tqa = zeros(2,1); % 作用于电机转子的等效力矩

J_theta_E = zeros(2,1); % 真实位置与期望位置误差
J_omega_E = zeros(2,1); % 真实速度与输入速度误差
J_theta_E_integral = zeros(2,1);

%% 初始化关节空间动力学参数矩阵
% 实际参数
J_M = [0,0;0,0];
J_V = [0,0;0,0];
J_B = [0,0;0,0];
J_G = [0;0];
% 理论参数
J_M_D = [0,0;0,0];
J_V_D = [0,0;0,0];
J_B_D = [0,0;0,0];
J_G_D = [0;0];

%% 设置加速度、速度和位置期望值
%% 位置保持
if (Traj_Type == Hold_on)
    J_theta_set = zeros(2,step);% 设置所有插补点处的关节期望转角，置为零
    J_omega_set = zeros(2,step);% 设置所有插补点处的关节期望速度，置为零
    J_epsilon_set = zeros(2,step);% 设置所有插补点处的关节期望速度，置为零
end
%% 位置斜坡轨迹
if (Traj_Type == Traj_Slope)
    [J_epsilon_set(1,:),J_omega_set(1,:),J_theta_set(1,:)] = Slope_traj(total_time,dt,pi/4,pi/4);
    [J_epsilon_set(2,:),J_omega_set(2,:),J_theta_set(2,:)] = Slope_traj(total_time,dt,pi/4,pi/4);
end
%% 位置S轨迹
if (Traj_Type == Traj_Pos_S)
    [J_epsilon_set(1,:), J_omega_set(1,:), J_theta_set(1,:)] = Position_S_traj(total_time,dt,pi/8);
    [J_epsilon_set(2,:), J_omega_set(2,:), J_theta_set(2,:)] = Position_S_traj(total_time,dt,pi/8);
end
%% 速度S轨迹
if (Traj_Type == Traj_Vol_S)
    [J_epsilon_set(1,:), J_omega_set(1,:), J_theta_set(1,:)] = Velocity_S_traj(total_time,dt,pi/20);
    [J_epsilon_set(2,:), J_omega_set(2,:), J_theta_set(2,:)] = Velocity_S_traj(total_time,dt,pi/20);
end

%% 主循环
for i = 1:step

    %% 根据连杆理论值和当前实际位置、速度更新动力学参数矩阵
    [J_M_D,J_V_D,J_B_D,J_G_D] = update_matrix(m_D, L_D, g, N, Ir, Bl, Br, J_real_theta_pre(1,1),J_real_theta_pre(2,1),J_real_omega_pre(1,1),J_real_omega_pre(2,1)); 
    
    %% 获得关节空间的控制期望值
    J_theta_d = J_theta_set(:,i); % 关节期望位置
    J_omega_d = J_omega_set(:,i); % 关节期望速度
    J_epsilon_d =J_epsilon_set(:,i); % 关节期望加速度 
    
    %% 计算关节位置和速度误差
    J_theta_E = J_theta_d - J_real_theta_pre; % 位置误差
    J_omega_E = J_omega_d - J_real_omega_pre; % 速度误差
    
     %% 调用逆动力学PID控制器，得到控制力矩
    J_theta_E_integral = J_theta_E_integral + J_theta_E .* dt;
    PID_epsilon = J_epsilon_d + Kp * J_theta_E + Kv * J_omega_E + Ki * J_theta_E_integral; % 指令加速度
    J_Tqc = J_M_D * PID_epsilon + J_V_D * J_real_omega_pre + J_B_D * J_real_omega_pre + J_G_D; % 关节空间控制力矩
    
    if (J_Tqc(1,1) > J_Tq_limit),T_qc(1,1) = J_Tq_limit; end % 控制力矩上限
    if (J_Tqc(2,1) > J_Tq_limit),T_qc(2,1) = J_Tq_limit; end % 控制电压上限
    if (J_Tqc(1,1) < -J_Tq_limit),Uc(1,1) = -J_Tq_limit; end % 控制电压上限
    if (J_Tqc(2,1) < -J_Tq_limit),Uc(2,1) = -J_Tq_limit; end % 控制电压上限
    
    
    %% 根据等效电机模型更新关节状态值，有两种方法：1. 根据角速度一阶微分方程的精确离散化模型计算角速度；2. 根据电机动力学方程直接计算角加速度，两种都有问题，在小减速比时不稳定
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
    % 根据平面2R机器人非线性力矩模型和实际参数，计算实际非线性力矩Tq_d，用于模拟干扰力矩
%     M_Tqd = P2R_Robot_Nonlinear_Inv_Dynamic(m,g,L,N,J_real_theta_pre, J_real_omega_pre, J_real_epsilon_pre);
%     M_Tqa = J_Tqc ./ N - M_Tqd; % 作用于电机转子的等效合力矩
% 
% %     M_real_omega_aft = (Mm_Average * inv(Bm))/(Mm_Average * inv(Bm) + dt .* eye(2))*M_real_omega_pre + (dt .* eye(2)*inv(Bm))* inv(Mm_Average * inv(Bm) + dt .* eye(2))*M_Tqa; % 利用电机动力学模型计算电机输出转速
% %     M_real_epsilon_aft = (M_real_omega_aft - M_real_omega_pre) ./ dt; % 电机实际角加速度
% 
%     M_real_epsilon_aft = Mm_Average \ (M_Tqa - Bm * M_real_omega_pre); % 根据电机的动力学模型直接计算加速度
%     M_real_omega_aft = M_real_omega_pre + M_real_epsilon_aft .* dt; % 电机实际角速度
% 
%     M_real_theta_aft = M_real_theta_pre + M_real_omega_aft .* dt; % 电机实际角度
%     J_real_omega_aft = M_real_omega_aft ./ N; % 关节实际角速度
%     J_real_theta_aft = M_real_theta_aft ./ N; % 关节实际角度
%     J_real_epsilon_aft = M_real_epsilon_aft ./ N; % 关节实际角加速度
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %% 根据关节空间动力学方程直接计算关节加速度，在小减速比时也能稳定，说明比力矩前馈补偿效果要好
    [J_M,J_V,J_B,J_G] = update_matrix(m, L, g, N, Ir, Bl, Br, J_real_theta_pre(1,1),J_real_theta_pre(2,1),J_real_omega_pre(1,1),J_real_omega_pre(2,1)); 
    J_real_epsilon_aft =  J_M \ (J_Tqc - (J_V * J_real_omega_pre + J_B * J_real_omega_pre + J_G));
    J_real_omega_aft = J_real_omega_pre +  J_real_epsilon_aft .* dt; % 关节实际角速度
    J_real_theta_aft = J_real_theta_pre + J_real_omega_aft .* dt; % 关节实际角加速度
    M_real_theta_aft = J_real_theta_aft .* N; % 电机实际角度
    M_real_omega_aft = J_real_omega_aft .* N; % 电机实际角速度
    M_real_epsilon_aft = J_real_epsilon_aft .* N;  % 电机实际角加速度
    
    J_theta(:,i) =  J_real_theta_aft; % 记录当前关节角度
    J_omega(:,i) =  J_real_omega_aft; % 记录当前关节角速度
    J_epsilon(:,i) =  J_real_epsilon_aft; % 记录当前关节角加速度
    
    %% 更新迭代前的电机和关节状态变量
    M_real_theta_pre = M_real_theta_aft; % 更新迭代前电机实际转角
    M_real_omega_pre = M_real_omega_aft; % 更新迭代前电机实际转速
    M_real_epsilon_pre = M_real_epsilon_aft; % 更新迭代前电机实际角加速度
    J_real_theta_pre = J_real_theta_aft; % 更新迭代前关节实际转角
    J_real_omega_pre = J_real_omega_aft; % 更新迭代前关节实际转速
    J_real_epsilon_pre = J_real_epsilon_aft; % 更新迭代前关节实际角加速度    
    
end

%% 绘图
%% 1#关节加速度曲线
figure;
yyaxis left;
% ylabel('ε(t)');
p = plot(time,J_epsilon(1,:),'k-',time,J_epsilon_set(1,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
if (Traj_Type == Traj_Pos_S), ylim([-3,3]);end
if (Traj_Type == Traj_Vol_S), ylim([-5,5]);end
set(gca,'ycolor','k');
yyaxis right;
% ylabel('ε_e(t)');
p = plot(time, J_epsilon_set(1,:) - J_epsilon(1,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-2,4]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("1#关节加速度响应曲线"));
%% 1#关节速度曲线
figure;
yyaxis left;
% ylabel('ω(t)');
p=plot(time,J_omega(1,:),'k-',time,J_omega_set(1,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
set(gca,'ycolor','k');
yyaxis right;
% ylabel('ω_e(t)');
p = plot(time, J_omega_set(1,:) - J_omega(1,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-0.06,0.2]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("1#关节速度响应曲线"));
%% 1#关节位置曲线
figure;
yyaxis left;
% ylabel('θ(t)');
p=plot(time,J_theta(1,:),'k-',time,J_theta_set(1,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
set(gca,'ycolor','k');
yyaxis right;
% ylabel('θ_e(t)');
p = plot(time, J_theta_set(1,:) - J_theta(1,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-0.005,0.045]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("1#关节位置响应曲线"));
%% 2#关节加速度曲线
figure;
yyaxis left;
% ylabel('ε(t)');
p = plot(time,J_epsilon(2,:),'k-',time,J_epsilon_set(2,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
if (Traj_Type == Traj_Pos_S), ylim([-3,3]);end
if (Traj_Type == Traj_Vol_S), ylim([-5,5]);end
set(gca,'ycolor','k');
yyaxis right;
% ylabel('ε_e(t)');
p = plot(time, J_epsilon_set(2,:) - J_epsilon(2,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-2,4]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("2#关节加速度响应曲线"));
%% 2#关节速度曲线
figure;
yyaxis left;
% ylabel('ω(t)');
p=plot(time,J_omega(2,:),'k-',time,J_omega_set(2,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
set(gca,'ycolor','k');
yyaxis right;
% ylabel('ω_e(t)');
p = plot(time, J_omega_set(2,:) - J_omega(2,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-0.06,0.2]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("2#关节速度响应曲线"));
%% 2#关节位置曲线
figure;
yyaxis left;
% ylabel('θ(t)');
p=plot(time,J_theta(2,:),'k-',time,J_theta_set(2,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
set(gca,'ycolor','k');
yyaxis right;
% ylabel('θ_e(t)');
p = plot(time, J_theta_set(2,:) - J_theta(2,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-0.005,0.045]);end
set(gca,'ycolor','k');
% xlabel('t');
title(sprintf("2#关节位置响应曲线"));